/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_util.h"

const int32_t Sin_Table[2048] = 
{
    0,50,101,151,201,251,302,352,402,452,503,553,603,653,704,754,804,854,904,955,1005,1055,1105,1155,1205,1255,1306,1356,1406,1456,1506,
    1556,1606,1656,1706,1756,1806,1856,1906,1956,2006,2055,2105,2155,2205,2255,2305,2354,2404,2454,2503,2553,2603,2652,2702,2752,2801,
    2851,2900,2949,2999,3048,3098,3147,3196,3246,3295,3344,3393,3442,3492,3541,3590,3639,3688,3737,3786,3835,3883,3932,3981,4030,4078,
    4127,4176,4224,4273,4321,4370,4418,4467,4515,4563,4612,4660,4708,4756,4804,4852,4900,4948,4996,5044,5092,5139,5187,5235,5282,5330,
    5377,5425,5472,5520,5567,5614,5661,5708,5756,5803,5850,5897,5943,5990,6037,6084,6130,6177,6223,6270,6316,6363,6409,6455,6501,6547,
    6593,6639,6685,6731,6777,6823,6868,6914,6960,7005,7050,7096,7141,7186,7231,7276,7321,7366,7411,7456,7501,7545,7590,7635,7679,7723,
    7768,7812,7856,7900,7944,7988,8032,8076,8119,8163,8207,8250,8293,8337,8380,8423,8466,8509,8552,8595,8638,8680,8723,8765,8808,8850,
    8892,8935,8977,9019,9061,9102,9144,9186,9227,9269,9310,9352,9393,9434,9475,9516,9557,9598,9638,9679,9720,9760,9800,9841,9881,9921,
    9961,10001,10040,10080,10120,10159,10198,10238,10277,10316,10355,10394,10433,10471,10510,10549,10587,10625,10663,10702,10740,10778,
    10815,10853,10891,10928,10966,11003,11040,11077,11114,11151,11188,11224,11261,11297,11334,11370,11406,11442,11478,11514,11550,11585,
    11621,11656,11691,11727,11762,11797,11831,11866,11901,11935,11970,12004,12038,12072,12106,12140,12173,12207,12240,12274,12307,12340,
    12373,12406,12439,12472,12504,12537,12569,12601,12633,12665,12697,12729,12760,12792,12823,12854,12885,12916,12947,12978,13008,13039,
    13069,13100,13130,13160,13190,13219,13249,13279,13308,13337,13366,13395,13424,13453,13482,13510,13538,13567,13595,13623,13651,13678,
    13706,13733,13761,13788,13815,13842,13869,13896,13922,13949,13975,14001,14027,14053,14079,14104,14130,14155,14181,14206,14231,14256,
    14280,14305,14329,14354,14378,14402,14426,14449,14473,14497,14520,14543,14566,14589,14612,14635,14657,14680,14702,14724,14746,14768,
    14789,14811,14832,14854,14875,14896,14917,14937,14958,14978,14999,15019,15039,15059,15078,15098,15118,15137,15156,15175,15194,15213,
    15231,15250,15268,15286,15304,15322,15340,15357,15375,15392,15409,15426,15443,15460,15476,15493,15509,15525,15541,15557,15573,15588,
    15604,15619,15634,15649,15664,15679,15693,15707,15722,15736,15750,15763,15777,15791,15804,15817,15830,15843,15856,15868,15881,15893,
    15905,15917,15929,15941,15952,15964,15975,15986,15997,16008,16018,16029,16039,16049,16059,16069,16079,16088,16098,16107,16116,16125,
    16134,16143,16151,16160,16168,16176,16184,16192,16199,16207,16214,16221,16228,16235,16242,16248,16255,16261,16267,16273,16279,16284,
    16290,16295,16300,16305,16310,16315,16319,16324,16328,16332,16336,16340,16343,16347,16350,16353,16356,16359,16362,16364,16367,16369,
    16371,16373,16375,16376,16378,16379,16380,16381,16382,16383,16383,16384,16384,16384,16384,16384,16383,16383,16382,16381,16380,16379,
    16378,16376,16375,16373,16371,16369,16367,16364,16362,16359,16356,16353,16350,16347,16343,16340,16336,16332,16328,16324,16319,16315,
    16310,16305,16300,16295,16290,16284,16279,16273,16267,16261,16255,16248,16242,16235,16228,16221,16214,16207,16199,16192,16184,16176,
    16168,16160,16151,16143,16134,16125,16116,16107,16098,16088,16079,16069,16059,16049,16039,16029,16018,16008,15997,15986,15975,15964,
    15952,15941,15929,15917,15905,15893,15881,15868,15856,15843,15830,15817,15804,15791,15777,15763,15750,15736,15722,15707,15693,15679,
    15664,15649,15634,15619,15604,15588,15573,15557,15541,15525,15509,15493,15476,15460,15443,15426,15409,15392,15375,15357,15340,15322,
    15304,15286,15268,15250,15231,15213,15194,15175,15156,15137,15118,15098,15078,15059,15039,15019,14999,14978,14958,14937,14917,14896,
    14875,14854,14832,14811,14789,14768,14746,14724,14702,14680,14657,14635,14612,14589,14566,14543,14520,14497,14473,14449,14426,14402,
    14378,14354,14329,14305,14280,14256,14231,14206,14181,14155,14130,14104,14079,14053,14027,14001,13975,13949,13922,13896,13869,13842,
    13815,13788,13761,13733,13706,13678,13651,13623,13595,13567,13538,13510,13482,13453,13424,13395,13366,13337,13308,13279,13249,13219,
    13190,13160,13130,13100,13069,13039,13008,12978,12947,12916,12885,12854,12823,12792,12760,12729,12697,12665,12633,12601,12569,12537,
    12504,12472,12439,12406,12373,12340,12307,12274,12240,12207,12173,12140,12106,12072,12038,12004,11970,11935,11901,11866,11831,11797,
    11762,11727,11691,11656,11621,11585,11550,11514,11478,11442,11406,11370,11334,11297,11261,11224,11188,11151,11114,11077,11040,11003,
    10966,10928,10891,10853,10815,10778,10740,10702,10663,10625,10587,10549,10510,10471,10433,10394,10355,10316,10277,10238,10198,10159,
    10120,10080,10040,10001,9961,9921,9881,9841,9800,9760,9720,9679,9638,9598,9557,9516,9475,9434,9393,9352,9310,9269,9227,9186,9144,
    9102,9061,9019,8977,8935,8892,8850,8808,8765,8723,8680,8638,8595,8552,8509,8466,8423,8380,8337,8293,8250,8207,8163,8119,8076,8032,
    7988,7944,7900,7856,7812,7768,7723,7679,7635,7590,7545,7501,7456,7411,7366,7321,7276,7231,7186,7141,7096,7050,7005,6960,6914,6868,
    6823,6777,6731,6685,6639,6593,6547,6501,6455,6409,6363,6316,6270,6223,6177,6130,6084,6037,5990,5943,5897,5850,5803,5756,5708,5661,
    5614,5567,5520,5472,5425,5377,5330,5282,5235,5187,5139,5092,5044,4996,4948,4900,4852,4804,4756,4708,4660,4612,4563,4515,4467,4418,
    4370,4321,4273,4224,4176,4127,4078,4030,3981,3932,3883,3835,3786,3737,3688,3639,3590,3541,3492,3442,3393,3344,3295,3246,3196,3147,
    3098,3048,2999,2949,2900,2851,2801,2752,2702,2652,2603,2553,2503,2454,2404,2354,2305,2255,2205,2155,2105,2055,2006,1956,1906,1856,
    1806,1756,1706,1656,1606,1556,1506,1456,1406,1356,1306,1255,1205,1155,1105,1055,1005,955,904,854,804,754,704,653,603,553,503,452,
    402,352,302,251,201,151,101,50,0,-50,-101,-151,-201,-251,-302,-352,-402,-452,-503,-553,-603,-653,-704,-754,-804,-854,-904,-955,-1005,
    -1055,-1105,-1155,-1205,-1255,-1306,-1356,-1406,-1456,-1506,-1556,-1606,-1656,-1706,-1756,-1806,-1856,-1906,-1956,-2006,-2055,-2105,
    -2155,-2205,-2255,-2305,-2354,-2404,-2454,-2503,-2553,-2603,-2652,-2702,-2752,-2801,-2851,-2900,-2949,-2999,-3048,-3098,-3147,-3196,
    -3246,-3295,-3344,-3393,-3442,-3492,-3541,-3590,-3639,-3688,-3737,-3786,-3835,-3883,-3932,-3981,-4030,-4078,-4127,-4176,-4224,-4273,
    -4321,-4370,-4418,-4467,-4515,-4563,-4612,-4660,-4708,-4756,-4804,-4852,-4900,-4948,-4996,-5044,-5092,-5139,-5187,-5235,-5282,-5330,
    -5377,-5425,-5472,-5520,-5567,-5614,-5661,-5708,-5756,-5803,-5850,-5897,-5943,-5990,-6037,-6084,-6130,-6177,-6223,-6270,-6316,-6363,
    -6409,-6455,-6501,-6547,-6593,-6639,-6685,-6731,-6777,-6823,-6868,-6914,-6960,-7005,-7050,-7096,-7141,-7186,-7231,-7276,-7321,-7366,
    -7411,-7456,-7501,-7545,-7590,-7635,-7679,-7723,-7768,-7812,-7856,-7900,-7944,-7988,-8032,-8076,-8119,-8163,-8207,-8250,-8293,-8337,
    -8380,-8423,-8466,-8509,-8552,-8595,-8638,-8680,-8723,-8765,-8808,-8850,-8892,-8935,-8977,-9019,-9061,-9102,-9144,-9186,-9227,-9269,
    -9310,-9352,-9393,-9434,-9475,-9516,-9557,-9598,-9638,-9679,-9720,-9760,-9800,-9841,-9881,-9921,-9961,-10001,-10040,-10080,-10120,-10159,
    -10198,-10238,-10277,-10316,-10355,-10394,-10433,-10471,-10510,-10549,-10587,-10625,-10663,-10702,-10740,-10778,-10815,-10853,-10891,-10928,
    -10966,-11003,-11040,-11077,-11114,-11151,-11188,-11224,-11261,-11297,-11334,-11370,-11406,-11442,-11478,-11514,-11550,-11585,-11621,-11656,
    -11691,-11727,-11762,-11797,-11831,-11866,-11901,-11935,-11970,-12004,-12038,-12072,-12106,-12140,-12173,-12207,-12240,-12274,-12307,-12340,
    -12373,-12406,-12439,-12472,-12504,-12537,-12569,-12601,-12633,-12665,-12697,-12729,-12760,-12792,-12823,-12854,-12885,-12916,-12947,-12978,
    -13008,-13039,-13069,-13100,-13130,-13160,-13190,-13219,-13249,-13279,-13308,-13337,-13366,-13395,-13424,-13453,-13482,-13510,-13538,-13567,
    -13595,-13623,-13651,-13678,-13706,-13733,-13761,-13788,-13815,-13842,-13869,-13896,-13922,-13949,-13975,-14001,-14027,-14053,-14079,-14104,
    -14130,-14155,-14181,-14206,-14231,-14256,-14280,-14305,-14329,-14354,-14378,-14402,-14426,-14449,-14473,-14497,-14520,-14543,-14566,-14589,
    -14612,-14635,-14657,-14680,-14702,-14724,-14746,-14768,-14789,-14811,-14832,-14854,-14875,-14896,-14917,-14937,-14958,-14978,-14999,-15019,
    -15039,-15059,-15078,-15098,-15118,-15137,-15156,-15175,-15194,-15213,-15231,-15250,-15268,-15286,-15304,-15322,-15340,-15357,-15375,-15392,
    -15409,-15426,-15443,-15460,-15476,-15493,-15509,-15525,-15541,-15557,-15573,-15588,-15604,-15619,-15634,-15649,-15664,-15679,-15693,-15707,
    -15722,-15736,-15750,-15763,-15777,-15791,-15804,-15817,-15830,-15843,-15856,-15868,-15881,-15893,-15905,-15917,-15929,-15941,-15952,-15964,
    -15975,-15986,-15997,-16008,-16018,-16029,-16039,-16049,-16059,-16069,-16079,-16088,-16098,-16107,-16116,-16125,-16134,-16143,-16151,-16160,
    -16168,-16176,-16184,-16192,-16199,-16207,-16214,-16221,-16228,-16235,-16242,-16248,-16255,-16261,-16267,-16273,-16279,-16284, -16290,-16295,
    -16300,-16305,-16310,-16315,-16319,-16324,-16328,-16332,-16336,-16340,-16343,-16347,-16350,-16353,-16356,-16359,-16362,-16364,-16367,-16369,
    -16371,-16373,-16375,-16376,-16378,-16379,-16380,-16381,-16382,-16383,-16383,-16384,-16384,-16384,-16384,-16384,-16383,-16383,-16382,-16381,
    -16380,-16379,-16378,-16376,-16375,-16373,-16371,-16369,-16367,-16364,-16362,-16359,-16356,-16353,-16350,-16347,-16343,-16340,-16336,-16332,
    -16328,-16324,-16319,-16315,-16310,-16305,-16300,-16295,-16290,-16284,-16279,-16273,-16267,-16261,-16255,-16248,-16242,-16235,-16228,-16221,
    -16214,-16207,-16199,-16192,-16184,-16176,-16168,-16160,-16151,-16143,-16134,-16125,-16116,-16107,-16098,-16088,-16079,-16069,-16059,-16049,
    -16039,-16029,-16018,-16008,-15997,-15986,-15975,-15964,-15952,-15941,-15929,-15917,-15905,-15893,-15881,-15868,-15856,-15843,-15830,-15817,
    -15804,-15791,-15777,-15763,-15750,-15736,-15722,-15707,-15693,-15679,-15664,-15649,-15634,-15619,-15604,-15588,-15573,-15557,-15541,-15525,
    -15509,-15493,-15476,-15460,-15443,-15426,-15409,-15392,-15375,-15357,-15340,-15322,-15304,-15286,-15268,-15250,-15231,-15213,-15194,-15175,
    -15156,-15137,-15118,-15098,-15078,-15059,-15039,-15019,-14999,-14978,-14958,-14937,-14917,-14896,-14875,-14854,-14832,-14811,-14789,-14768,
    -14746,-14724,-14702,-14680,-14657,-14635,-14612,-14589,-14566,-14543,-14520,-14497,-14473,-14449,-14426,-14402,-14378,-14354,-14329,-14305,
    -14280,-14256,-14231,-14206,-14181,-14155,-14130,-14104,-14079,-14053,-14027,-14001,-13975,-13949,-13922,-13896,-13869,-13842,-13815,-13788,
    -13761,-13733,-13706,-13678,-13651,-13623,-13595,-13567,-13538,-13510,-13482,-13453,-13424,-13395,-13366,-13337,-13308,-13279,-13249,-13219,
    -13190,-13160,-13130,-13100,-13069,-13039,-13008,-12978,-12947,-12916,-12885,-12854,-12823,-12792,-12760,-12729,-12697,-12665,-12633,-12601,
    -12569,-12537,-12504,-12472,-12439,-12406,-12373,-12340,-12307,-12274,-12240,-12207,-12173,-12140,-12106,-12072,-12038,-12004,-11970,-11935,
    -11901,-11866,-11831,-11797,-11762,-11727,-11691,-11656,-11621,-11585,-11550,-11514,-11478,-11442,-11406,-11370,-11334,-11297,-11261,-11224,
    -11188,-11151,-11114,-11077,-11040,-11003,-10966,-10928,-10891,-10853,-10815,-10778,-10740,-10702,-10663,-10625,-10587,-10549,-10510,-10471,
    -10433,-10394,-10355,-10316,-10277,-10238,-10198,-10159,-10120,-10080,-10040,-10001,-9961,-9921,-9881,-9841,-9800,-9760,-9720,-9679,-9638,
    -9598,-9557,-9516,-9475,-9434,-9393,-9352,-9310,-9269,-9227,-9186,-9144,-9102,-9061,-9019,-8977,-8935,-8892,-8850,-8808,-8765,-8723,-8680,
    -8638,-8595,-8552,-8509,-8466,-8423,-8380,-8337,-8293,-8250,-8207,-8163,-8119,-8076,-8032,-7988,-7944,-7900,-7856,-7812,-7768,-7723,-7679,
    -7635,-7590,-7545,-7501,-7456,-7411,-7366,-7321,-7276,-7231,-7186,-7141,-7096,-7050,-7005,-6960,-6914,-6868,-6823,-6777,-6731,-6685,-6639,
    -6593,-6547,-6501,-6455,-6409,-6363,-6316,-6270,-6223,-6177,-6130,-6084,-6037,-5990,-5943,-5897,-5850,-5803,-5756,-5708,-5661,-5614,-5567,
    -5520,-5472,-5425,-5377,-5330,-5282,-5235,-5187,-5139,-5092,-5044,-4996,-4948,-4900,-4852,-4804,-4756,-4708,-4660,-4612,-4563,-4515,-4467,
    -4418,-4370,-4321,-4273,-4224,-4176,-4127,-4078,-4030,-3981,-3932,-3883,-3835,-3786,-3737,-3688,-3639,-3590,-3541,-3492,-3442,-3393,-3344,
    -3295,-3246,-3196,-3147,-3098,-3048,-2999,-2949,-2900,-2851,-2801,-2752,-2702,-2652,-2603,-2553,-2503,-2454,-2404,-2354,-2305,-2255,-2205,
    -2155,-2105,-2055,-2006,-1956,-1906,-1856,-1806,-1756,-1706,-1656,-1606,-1556,-1506,-1456,-1406,-1356,-1306,-1255,-1205,-1155,-1105,-1055,
    -1005,-955,-904,-854,-804,-754,-704,-653,-603,-553,-503,-452,-402,-352,-302,-251,-201,-151,-101,-50
};
/**
 * @description: 查表获取cos值，表格是将2π分割为2048份，表格值为sin，cos相差π/2，因此移动了1/4（2048/4 = 512）的位置
 * @return {*} 返回扩大了16384倍的cos值
 * @param {int32_t} pahse 弧度值（0~2π）
 */
int32_t arm_cos_f32(int pahse)
{
    uint16_t num;
	num = 1024 * pahse / 3142 + 512;
	num = num % 2048;
	if(num < 0)
		num += 2048;
	return Sin_Table[num];
}
/**
 * @description: 查表获取cos值，表格是将2π分割为2048份
 * @return {*} 返回扩大了16384倍的cos值
 * @param {int} pahse 弧度值（0~2π）
 */
int32_t arm_sin_f32(int pahse)
{
    uint16_t num;
	num = 1024 * pahse / 3142;
	num = num % 2048;
	if(num < 0)
		num += 2048;
	return Sin_Table[num];
}

float MagicCubeRoot(float x)
{
    float xthird = 0.333f * x;
    int i = *(int*)&x;
    i = (0x2a517d47 + (0.333f * i));
    x = *(float*)&i;
    x = 0.667f * x + xthird / (x * x);
    x = 0.667f * x + xthird / (x * x);
    return x;
}

float MagicSqrt(float x)
{
    float xhalf = 0.5f * x;
    int i = *(int*)&x;
    i = 0x1fbd1df5 + (i >> 1);
    x = *(float*)&i;
    x = 0.5f * x + xhalf / x;
    return x;
}
